
#include <stdio.h>
#include <math.h>
#include <ebex_quaternion.h>
#include <ebex_vector.h>
#include <ebex_matrix.h>

int main(void)
{
    ebex_3vec_t zero_vec;
    ebex_3vec_t one_vec;
    ebex_3vec_t diff_vec;

    ebex_quat_t zero_quat;
    ebex_quat_t one_quat;
    ebex_quat_t diff_quat;

    ebex_mat3x3_t tmp_mat;

    zero_vec.yaw = 0.0335042985047;
    zero_vec.pitch = 0.904187396618;
    zero_vec.roll = 0.0;
    one_vec.yaw = 0.026500482522;
    one_vec.pitch = 0.905774525327;
    one_vec.roll = 0.0;


    ebex_quat_from_euler(&zero_quat, &zero_vec, rotation_order_ZYX);
    ebex_quat_from_euler(&one_quat, &one_vec, rotation_order_ZYX);
    printf("Roll: %f\tPitch: %f\tYaw: %f\n", zero_vec.roll, zero_vec.pitch, zero_vec.yaw);

    ebex_quat_error(&diff_quat, &one_quat, &zero_quat);
    ebex_matrix_from_quat(&tmp_mat, &diff_quat);
    ebex_3vec_euler_from_3mat(&diff_vec, rotation_order_ZYX, &tmp_mat);
    printf("Roll: %f\tPitch: %f\tYaw: %f\n", diff_vec.roll, diff_vec.pitch, diff_vec.yaw);

	ebex_quat_propagate(&zero_quat, &one_quat, &diff_vec, 1.0);
	ebex_quat_normalize(&zero_quat, &zero_quat);
    ebex_matrix_from_quat(&tmp_mat, &zero_quat);
    ebex_3vec_euler_from_3mat_ref(&diff_vec, &zero_vec, rotation_order_ZYX, &tmp_mat);
    printf("Roll: %f\tPitch: %f\tYaw: %f\n", diff_vec.roll, diff_vec.pitch, diff_vec.yaw);

    return EXIT_SUCCESS;
}
